#!/usr/bin/env python
"""Recieves velocity commands (subscribes to some topic transmitting desired velocity) and sends them to the motors"""
import roslib; roslib.load_manifest('smc_04b')
import rospy
import serial
from std_msgs.msg import String

class SMC04Bdrv:

	global cmdVel # desired velocity
	

	def __init__(self, cmdVel):
		self.cmdVel = cmdVel
	
	def get_up_to_speed(self);
		# get actual velocity from the node published 
		# if actVel < cmdVel, increase motor speed command by 1 
		# else, if actVel > cmdVel, decrease motor speed command by 1
		# or do nothing
		# keep doing this while checking speed until actVel = cmdVel +/- (errormargin)*(cmdVel)
		while actVel < cmdVel 	# account for error margin? Close-enough margin? 5%? 
			increase_velocity(self)
		while actVel > cmdVel:
			decrease_velocity(self)

	def increase_velocity(self):
		# self.wheel1vel =+ (so long as not maxed out)
		# self.wheel2vel =+ (same condition)
	def decrease_velocity(self):
		# self.wheel1vel -= 1
		# self.wheel2vel -= 1


	def interp_keys(self, keydata):
		key = keydata.data
		print keydata 	
		print key
	
		m1Speed = self.m1Speed
		m2Speed = self.m2Speed
		print "Beginning motor speeds are [" + str(m2Speed) + ', ' + str(m1Speed) + "] "

		if (key=="S") or (key=="s"):
			print "Emergency Stop"
			m1Speed = 0
			m2Speed = 0	
		elif key=="q":
			print "Other key pressed - stopping motors and quitting"
			m1Speed = 0
			m2Speed = 0
			# Placeholder - need a program that decreases motor speeds gradually to come to a complete stop - less acceleration that way 
		elif (key=="U") or (key=="u"):
			print "U - go forward"
			if m1Speed == 128:
				pass #Ignore command if motor speed already at limit
			elif m2Speed == 128:
				pass
			else:
				m1Speed = m1Speed + 1
				m2Speed = m2Speed + 1
				# Increase speed of both wheels if possible
		elif (key=="J") or (key=="j"):
			print "J - Slow down"
			if (m1Speed == 0) and (m2Speed == 0):
				pass # Ignore command if motor speed already at limit
			elif m1Speed == 0:
				m2Speed = m2Speed - 1
			elif m2Speed == 0:
				m1Speed = m1Speed - 1
			else:		
				m1Speed = m1Speed - 1
				m2Speed = m2Speed - 1
				# Decrease speed of both wheels if possible
		elif (key=="H") or (key=="h"):
			print "H - Turn left"
			if (m1Speed==128) and (m2Speed==0):
				pass
			elif  m1Speed == 128:
				m2Speed = m2Speed - 1
			elif  m2Speed == 0:
				m1Speed = m1Speed + 1		
			else:
				m1Speed = m1Speed + 1
				m2Speed = m2Speed - 1
				# Turn left: increase right and decrease left, or just decrease left. If already at limit, ignore command
		elif (key=="K") or (key=="k"):
			print "K - Turn right"	
			if (m2Speed==128) and (m1Speed==0):
				pass
			elif m2Speed == 128:
				m1Speed = m1Speed - 1
			elif  m1Speed == 0:
				m2Speed = m2Speed + 1			
			else:
				m2Speed = m2Speed + 1
				m1Speed = m1Speed - 1		
				# Turn Right: increase left speed and decrease right speed, or just decrease right.. If already at limit, ignore command
		print "Key recieved, passing motor speeds to compose_motor_cmd"
		self.compose_motor_cmd(m1Speed, m2Speed)
		# Pass speed info to method which produces the motor command 
		print "Re-assigning motor speeds for next loop"
		self.m1Speed = m1Speed
		self.m2Speed = m2Speed
		print "Final motor speeds are  [" + str(m2Speed) + ', ' + str(m1Speed) + "] "

	def compose_motor_cmd(self, m1Speed, m2Speed):
		cmd1 = [0x80, 0x00, 0x03, m1Speed]
		cmd2 = [0x80, 0x00, 0x04, m2Speed]
		# Compose hex commands (in python, these are strings)
		print "Sent motor speeds are [" + str(m2Speed) + ', ' + str(m1Speed) + "] "
		#self.cmd_to_m1(cmd1)
		#self.cmd_to_m2(cmd2)
		# Send commands to respective motor's serial port
	def cmd_to_m1(self, cmd):
		m1 = serial.Serial('/dev/robot/motor1', 9600)
		for x in cmd:
			m1.write(chr(x))

	def cmd_to_m2(self, cmd):
		m2 = serial.Serial('/dev/robot/motor2', 9600)
		for x in cmd:
			m2.write(chr(x))	

	def teleop_key_driver(self):
		print "Initializing 'teleop_key_driver' node"
		rospy.init_node('teleop_key_driver', anonymous=True)
		print "Beginning subscribing, passing data to interp_keys"
		teleop_key_sub = rospy.Subscriber("teleop_key", String, 	self.interp_keys)
		rospy.spin()

if __name__ == '__main__':
	print "Creating instance of SMC04Bdrv class"
	drv = SMC04Bdrv(0,0)	
	print "Calling teleop_key_driver()"	
	drv.teleop_key_driver()
